from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():

    wall_localization_node = Node(
        package='wall_localization', 
        executable='wall_localizer', 
        name='wall_localizer',
        output='screen',
        parameters=[
            PathJoinSubstitution([                    
                get_package_share_directory('wall_localization'), 
                'config',
                'wall_localizer.params.yaml'
            ])   
        ]
    )

    wearhouse_navigator_node = Node(
        package='wearhouse_navigation',
        executable='wearhouse_navigator',
        name='wearhouse_navigator',
        output='screen',
        parameters=[
            PathJoinSubstitution([ 
                get_package_share_directory('wearhouse_navigation'), 
                'config',
                'wearhouse_navigator.params.yaml'
            ]),
            PathJoinSubstitution([ 
                get_package_share_directory('wearhouse_navigation'),
                'config',
                'goals.yaml'
            ])
        ]
    )


    return LaunchDescription([
        wall_localization_node,
        wearhouse_navigator_node,
    ])

    